#include "dbc_parser.h"
#include <istream>
#include <sstream>
#include <limits>
#include <iterator>
#include <algorithm>
#include <vector>
#include <iosfwd>
#include <fstream>
#include <cmath>

namespace DBC
{
//==============================================================================================================
//==================================     DBC Signal parted              ========================================
//==============================================================================================================
DBCSignal::DBCSignal(DBCSignal&& sig) noexcept :
    name_(std::move(sig.name_)),
    order_(sig.order_),
    start_bit_(sig.start_bit_),
    length_(sig.length_),
    sign_(sig.sign_),
    minimum_(sig.minimum_),
    maximum_(sig.maximum_),
    factor_(sig.factor_),
    offset_(sig.offset_),
    unit_(std::move(sig.unit_)),
    multiplexor_(sig.multiplexor_),
    multiplexor_num_(sig.multiplexor_num_),
    to_list_(std::move(sig.to_list_))
{
}

std::string& trim(std::string& str, const std::string& to_trim = " ")
{
    std::string::size_type pos = str.find_last_not_of(to_trim);
    if (pos == std::string::npos)
        str.clear();
    else
    {
        str.erase(pos + 1);
        str.erase(0, str.find_first_not_of(to_trim));
    }
    return str;
}

std::vector<std::string>& split(const std::string &s, char delim, std::vector<std::string> &elems) {
    std::stringstream ss(s);
    std::string item;
    while (std::getline(ss, item, delim)) {
        elems.push_back(item);
    }
    return elems;
}

std::vector<std::string> split(const std::string &s, char delim)
{
    std::vector<std::string> elems;
    split(s, delim, elems);
    return elems;
}

std::istream& operator>>(std::istream& is, DBCSignal& sig)
{
    int c = is.peek();
    if ('B' == c)
    {
        is.setstate(std::ios_base::failbit);
        return is;
    }
    // 读取一整条 dbc 报文进行解析
    std::string line;
    std::getline(is, line);
    if (!line.empty() && *line.rbegin() == '\r')
        line.erase(line.length() - 1, 1);
    if (line.empty())
    {
        is.setstate(std::ios_base::failbit);
        return is;
    }
    std::istringstream sstream(line);
    std::string preamble;
    sstream >> preamble;
    // 检查读取内容是否为信号，否则跳过
    if (preamble != "SG_")
    {
        sstream.setstate(std::ios_base::failbit);
        return is;
    }
    // 解析信号名称
    sstream >> sig.name_;
    // 是否配置多路复用器
    std::string multi;
    sstream >> multi;
    if (multi == ":")
        sig.multiplexor_ = Multiplexor::NONE;
    else
    {
        if (multi == "M")
            sig.multiplexor_ = Multiplexor::MULTIPLEXOR;
        else
        {
            std::istringstream iss(multi);
            iss.ignore(1);
            unsigned short multi_num;
            iss >> multi_num;
            sig.multiplexor_ = Multiplexor::MULTIPLEXED;
            sig.multiplexor_num_ = multi_num;
        }
        sstream >> multi;
    }
    // 去读开始位
    sstream >> sig.start_bit_;
    sstream.ignore(1);
    // 信号长度
    sstream >> sig.length_;
    sstream.ignore(1);
    // 读取字节序
    int order;
    sstream >> order;
    if (order == 0)
        sig.order_ = ByteOrder::MOTOROLA;
    else
        sig.order_ = ByteOrder::INTEL;
    // 信号类型
    char sign;
    sstream >> sign;
    if (sign == '+')
        sig.sign_ = Sign::UNSIGNED;
    else
        sig.sign_ = Sign::SIGNED;

    sstream.ignore(std::numeric_limits<std::streamsize>::max(), '(');
    sstream >> sig.factor_;
    sstream.ignore(1);
    sstream >> sig.offset_;
    sstream.ignore(1);

    sstream.ignore(std::numeric_limits<std::streamsize>::max(), '[');
    sstream >> sig.minimum_;
    sstream.ignore(1);
    sstream >> sig.maximum_;
    sstream.ignore(1);

    std::string unit;
    sstream >> unit;
    sig.unit_ = trim(unit, "\"");

    std::string to;
    sstream >> to;
    std::vector<std::string> to_string = split(to, ',');
    std::move(to_string.begin(), to_string.end(), std::inserter(sig.to_list_, sig.to_list_.begin()));

    return is;
}


//==============================================================================================================
//==================================     DBC Message parted             ========================================
//==============================================================================================================

Message::Message(Message&& msg) noexcept
    : name_(std::move(msg.name_)),
      id_(msg.id_),
      dlc_(msg.dlc_),
      from_(std::move(msg.from_)),
      signals_(std::move(msg.signals_))
{
}

Message& Message::operator=(Message&& other) noexcept {
    if (this != &other) {
        name_     = std::move(other.name_);
        id_       = other.id_;
        dlc_      = other.dlc_;
        from_     = std::move(other.from_);
        signals_  = std::move(other.signals_);
    }
    return *this;
}

std::istream& operator>>(std::istream& is, Message& msg)
{
    std::string preamble;
    is >> preamble;
    if (preamble != "BO_")
    {
        is.setstate(std::ios_base::failbit);
        return is;
    }
    is >> msg.id_;
    std::string name;
    is >> name;
    msg.name_ = name.substr(0, name.length() - 1);
    is >> msg.dlc_;
    is >> msg.from_;
    is.ignore(std::numeric_limits<std::streamsize>::max(), '\n');

    while (is)
    {
        DBCSignal sig;
        is >> sig;
        if (is)
            msg.signals_.push_back(std::move(sig));
    }
    is.clear();
    return is;
}

std::set<std::string> Message::get_to() const
{
    std::set<std::string> collection;
    for (const auto& sig : signals_)
    {
        auto to_list = sig.get_to();
        collection.insert(to_list.begin(), to_list.end());
    }
    return collection;
}


//==============================================================================================================
//==================================     DBC Parser parted              ========================================
//==============================================================================================================
DBCParser::DBCParser(const std::string& file_path)
{
    std::ifstream file(file_path);
    if (file)
        init(file);
    else
        throw std::invalid_argument("DBC::DBCIT::DBCIT");
    file.close();
}

DBCParser::DBCParser(std::istream& stream)
{
    init(stream);
}

void DBCParser::init(std::istream& stream)
{
    message_map_.clear();
    std::vector<Message> messages;
    do
    {
        Message msg;
        stream >> msg;
        if (stream.fail())
        {
            stream.clear();
            stream.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
        }
        else
        {
            message_map_[msg.get_id()] = std::move(msg);
        }
    } while (!stream.eof());
}

std::vector<double> DBCParser::parser_message(uint32_t id, const uint8_t* data, size_t data_length)
{
    if (!data) return {};

    std::vector<double> result;
    auto it = message_map_.find(id);
    if (it == message_map_.end()) return {};
    const Message& message = it->second;

    // Helpers for bit extraction
    auto extract_intel = [&](unsigned short start_bit,
                             unsigned short length) -> uint64_t {
        if (length == 0) return 0ULL;
        uint64_t raw = 0ULL;
        for (unsigned short i = 0; i < length; ++i) {
            auto bit_index = static_cast<unsigned short>(start_bit + i);
            auto byte_index = static_cast<unsigned short>(bit_index / 8);
            auto bit_in_byte = static_cast<unsigned short>(bit_index % 8);
            uint8_t bit = 0;
            if (byte_index < data_length) {
                bit = static_cast<uint8_t>((data[byte_index] >> bit_in_byte) & 0x1);
            }
            raw |= (static_cast<uint64_t>(bit) << i);
        }
        return raw;
    };

    auto extract_motorola = [&](unsigned short start_bit,
                                unsigned short length) -> uint64_t {
        if (length == 0) return 0ULL;
        uint64_t raw = 0ULL;
        int byte_index = static_cast<int>(start_bit / 8);
        int bit_index_in_byte = static_cast<int>(start_bit % 8);
        for (unsigned short i = 0; i < length; ++i) {
            uint8_t bit = 0;
            if (byte_index >= 0 && byte_index < static_cast<int>(data_length)) {
                bit = static_cast<uint8_t>((data[byte_index] >> (7 - bit_index_in_byte)) & 0x1);
            }
            raw = (raw << 1) | static_cast<uint64_t>(bit);
            if (bit_index_in_byte == 0) {
                bit_index_in_byte = 7;
                ++byte_index;
            } else {
                --bit_index_in_byte;
            }
        }
        return raw;
    };

    auto scale_value = [](int64_t signed_val, double factor, double offset) -> double {
        return static_cast<double>(signed_val) * factor + offset;
    };

    auto scale_value_u = [](uint64_t unsigned_val, double factor, double offset) -> double {
        return static_cast<double>(unsigned_val) * factor + offset;
    };

    // Determine multiplexor selector if present
    bool has_selector = false;
    uint64_t selector_value = 0ULL;
    for (const auto& sig : message) {
        if (sig.get_multiplexor() == Multiplexor::MULTIPLEXOR) {
            has_selector = true;
            uint64_t raw = (sig.get_byte_order() == ByteOrder::INTEL)
                           ? extract_intel(sig.get_startbit(), sig.get_length())
                           : extract_motorola(sig.get_startbit(), sig.get_length());
            selector_value = raw;
            break;
        }
    }

    for (const auto& sig : message) {
        // Skip multiplexed signals that do not match selector
        if (sig.get_multiplexor() == Multiplexor::MULTIPLEXED && has_selector) {
            if (static_cast<uint64_t>(sig.get_multiplexed_num()) != selector_value) {
                continue;
            }
        }
        uint64_t raw = (sig.get_byte_order() == ByteOrder::INTEL)
                       ? extract_intel(sig.get_startbit(), sig.get_length())
                       : extract_motorola(sig.get_startbit(), sig.get_length());

        double phys = 0.0;
        if (sig.get_sign() == Sign::SIGNED) {
            unsigned short len = sig.get_length();
            int64_t signed_val;
            if (len == 64) {
                signed_val = static_cast<int64_t>(raw);
            } else {
                uint64_t sign_bit = 1ULL << (len - 1);
                if (raw & sign_bit) {
                    // negative number: sign extend
                    uint64_t mask = (~0ULL) << len;
                    signed_val = static_cast<int64_t>(raw | mask);
                } else {
                    signed_val = static_cast<int64_t>(raw);
                }
            }
            phys = scale_value(signed_val, sig.get_factor(), sig.get_offset());
        } else {
            phys = scale_value_u(raw, sig.get_factor(), sig.get_offset());
        }

        // Optional clamp to [min, max]
        if (phys < sig.get_minimum()) phys = sig.get_minimum();
        if (phys > sig.get_maximum()) phys = sig.get_maximum();

        result.push_back(phys);
    }
    return result;
}

bool DBCParser::create_message(uint32_t id, const std::vector<double>& phys_values,
                               uint8_t* data, size_t data_length)
{
    if (!data) return false;
    std::fill(data, data + data_length, 0); // 清零

    auto it = message_map_.find(id);
    if (it == message_map_.end()) return false;
    const Message& message = it->second;
    auto& signals = message.get_signals();

    if (phys_values.size() != signals.size()) return false; // 信号数量不匹配

    auto insert_intel = [&](unsigned short start_bit, unsigned short length, uint64_t raw) {
        for (unsigned short i = 0; i < length; ++i) {
            auto bit_index = static_cast<unsigned short>(start_bit + i);
            auto byte_index = static_cast<unsigned short>(bit_index / 8);
            auto bit_in_byte = static_cast<unsigned short>(bit_index % 8);
            if (byte_index < data_length) {
                data[byte_index] &= ~(1u << bit_in_byte); // 清零位
                data[byte_index] |= ((raw >> i) & 0x1u) << bit_in_byte;
            }
        }
    };

    auto insert_motorola = [&](unsigned short start_bit, unsigned short length, uint64_t raw) {
        int byte_index = start_bit / 8;
        int bit_index_in_byte = start_bit % 8;
        for (unsigned short i = 0; i < length; ++i) {
            if (byte_index >= 0 && byte_index < static_cast<int>(data_length)) {
                data[byte_index] &= ~(1u << (7 - bit_index_in_byte)); // 清零位
                data[byte_index] |= ((raw >> (length - 1 - i)) & 0x1u) << (7 - bit_index_in_byte);
            }
            if (bit_index_in_byte == 0) {
                bit_index_in_byte = 7;
                ++byte_index;
            } else {
                --bit_index_in_byte;
            }
        }
    };

    // 遍历信号并编码
    for (size_t idx = 0; idx < message.get_signals().size(); ++idx) {
        const auto& sig = signals[idx];
        double phys = phys_values[idx];

        // 反推 raw 值
        double temp = (phys - sig.get_offset()) / sig.get_factor();
        uint64_t raw;
        if (sig.get_sign() == Sign::SIGNED) {
            int64_t sval = std::llround(temp);
            raw = static_cast<uint64_t>(sval) & ((1ULL << sig.get_length()) - 1);
        } else {
            raw = static_cast<uint64_t>(std::llround(temp)) & ((1ULL << sig.get_length()) - 1);
        }

        if (sig.get_byte_order() == ByteOrder::INTEL) {
            insert_intel(sig.get_startbit(), sig.get_length(), raw);
        } else {
            insert_motorola(sig.get_startbit(), sig.get_length(), raw);
        }
    }

    return true;
}
}